#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/ioctl.h>
#include <sys/stat.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>

#include "bmi088.h"
//#include "xag_types.h"

/**
******************************************************************************
* @file           : bmi088-gyroscope.c
* @brief          : implement for bmi088-gyroscope.c file.
*                   This file contains the common implement of the application.
* @author		  : ac
* @attention
*
*		Copyright (c) 2021 Technology Co., Ltd.
*
*******************************************************************************
*/

/*
 * globla variable
 */



/*
 * function declaration
 */

/**
 * xag_spi_read_gyroscope - spi read operation.
 * @reg_addr : spi register address
 * @data : data buffer
 * @len : read length
 */
static XAG_S8 xag_spi_read_gyroscope(XAG_U8 reg_addr, XAG_U8 * data, XAG_U16 len)
{
	struct spi_ioc_transfer mesg[1];
	XAG_S32 retval = 0;
	XAG_S32 i = 0, index = 0;
	XAG_S32 tmp = 0;
	XAG_S32 fd = -1;
	XAG_S8 file_name[0x20] = { 0 };
	XAG_U8 buf[0x20] = { 0 };
	XAG_S32 cur_reg_addr; 
	XAG_S32 num_reg = 1, reg_width = 1, data_width = 1; 
    XAG_U8 temp_value[6] = { 0 };

	sprintf(file_name, "/dev/spidev%u.%u", 0, 0);

	fd = open(file_name, 0);
	if (fd < 0) {
		__do_abnormal("gyroscope spi open of read operation");
	}

	tmp = SPI_MODE_3;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval) {
		__do_abnormal("spi wr mode of read");
	}

	memset(mesg, 0, sizeof mesg);
	mesg[0].tx_buf = (__u64)buf;
	mesg[0].rx_buf = (__u64)buf;
	mesg[0].len = reg_width + data_width; 
	mesg[0].speed_hz = 5000000;  
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;

	memset(buf, 0, sizeof buf);
	memset(temp_value, 0, sizeof temp_value); 
  
	for (cur_reg_addr = reg_addr, i = 0; cur_reg_addr < reg_addr + len; cur_reg_addr++, i++) {
		index = 0; 
        
		if (reg_width == 1) {
			*(__u8 *)(&buf[index]) = (cur_reg_addr | 0x80) & 0xff;
			index++;
		}

		if (data_width == 1) {
			*(__u8 *)(&buf[index]) = 0x00;
		}

		retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
		if (retval != (XAG_S32)mesg[0].len) {
			printf("SPI_IOC_MESSAGE error \n");
			retval = -1;
			goto end1;
		}
		
		if (data_width == 1) {
			*(data+i) = *(__u8 *)(&buf[index]);
			retval = 0;
		}
	}

end1:
	close(fd);

end0:
	return retval;
}

/**
 * xag_spi_write_gyroscope - spi write operation.
 * @reg_addr : spi register address
 * @data : data buffer
 * @len : read length
 */
static XAG_S8 xag_spi_write_gyroscope(XAG_U8 reg_addr, XAG_U8 * data, uint16_t len)
{

	XAG_S32 retval = 0;
	XAG_S32  index = 0;
	XAG_S32 tmp = 0;
	XAG_S32 fd = -1;
	XAG_S8 file_name[0x20];
	XAG_U8 buf[0x10] = {0};
	struct spi_ioc_transfer mesg[1];
	XAG_U32  reg_width = 1, data_width = 1, reg_order = 1, data_order = 1;  //dev_width = 1,

	sprintf(file_name, "/dev/spidev%u.%u", 0, 0);

	fd = open(file_name, 0);
	if (fd < 0) {
		__do_abnormal("gyroscope spi open of write operation");
	}

	tmp = SPI_MODE_3;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval) {
		__do_abnormal("spi wr mode of write");
	}

	memset(mesg, 0, sizeof mesg);
	mesg[0].tx_buf = (__u64)buf;
	mesg[0].rx_buf = (__u64)buf;
	mesg[0].len = reg_width + data_width; 
	mesg[0].speed_hz = 5000000;
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;
	usleep(2000); 
	memset(buf, 0, sizeof buf);

	if (reg_width == 1) {
		*(__u8 *)(&buf[index]) = reg_addr & 0xff;
		index++;
	}

	if (data_width == 1)
		*(XAG_U8 *)(&buf[index]) = (*data) & 0xff;

	retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
	if (retval != (XAG_S32)mesg[0].len) {
		printf("SPI_IOC_MESSAGE error \n");
		retval = -1;
		goto end1;
	}

	retval = 0;

end1:
	close(fd);

end0:
	return retval;
}

/**
 * xag_user_get_gyro_data - get gyroscope sensor data.
 * @imu_gryoscope_sensor_data : sensor data x, y, z
 */
static XAG_S8 xag_user_get_gyro_data(struct imu_gryoscope_sensor_data * gryo_sensor_data)
{
    XAG_U8 index = 0, reg_addr, data[6] = {0};
    uint32_t lsb, msb, msblsb;
	XAG_S32 ret = -1;
	
	reg_addr = BMI088_GYRO_X_LSB_REG;
	ret = xag_spi_read_gyroscope(reg_addr, data, 6);
	if (ret == 0) {
		/* data in X axis */
		lsb = (uint32_t)data[index++];
		msb = (uint32_t)data[index++];
		msblsb = (msb << 8) | lsb;
		gryo_sensor_data->x = (int16_t)msblsb;	

		/* data in Y axis */
		lsb = (uint32_t)data[index++];
		msb = (uint32_t)data[index++];
		msblsb = (msb << 8) | lsb;
		gryo_sensor_data->y = (int16_t)msblsb; 

		/* data in Z axis */
		lsb = (uint32_t)data[index++];
		msb = (uint32_t)data[index++];
		msblsb = (msb << 8) | lsb;
		gryo_sensor_data->z = (int16_t)msblsb; 
	}

	return ret;
}

/**
 * xag_user_read_gyroscope - read gyroscope.
 *	@none
 */
XAG_VOID xag_user_read_gyroscope()
{
	XAG_U8 buf[0x10] = {0};
	XAG_U8 range = 0;
	XAG_U8 odr = 0;
	struct imu_gyroscope gryo[2] = {
		[0] = {
			.gyro_x_deg = 0.0,
			.gyro_y_deg = 0.0, 
			.gyro_z_deg = 0.0,
		},
		{
			.gyro_x_deg = 0.0,
			.gyro_y_deg = 0.0, 
			.gyro_z_deg = 0.0,
		}
	};
	struct imu_gryoscope_sensor_data gryo_sensor_data = {
		.x = 0, 
		.y = 0,
		.z = 0
	};

	/* spi test for read ID */
	xag_spi_read_gyroscope(0x00, buf, 1);
	printf("gryoscope ID is [%02X]...\n", buf[0]);

	/* set gyroscope range */
	range = BMI088_GYRO_RANGE_125_DPS;
	xag_spi_write_gyroscope(BMI088_GYRO_RANGE_REG, &range, 1);
	xag_spi_read_gyroscope(BMI088_GYRO_RANGE_REG, buf, 1);

	/* set gyroscope odr */
	odr = BMI088_GYRO_BW_23_ODR_200_HZ;
	xag_spi_write_gyroscope(BMI088_GYRO_BANDWIDTH_REG, &odr, 1);

	/* operation */
	while (1) {
		/* calculation gryoscope data */
		if (xag_user_get_gyro_data(&gryo_sensor_data) == 0) {
			gryo[0].gyro_x_deg = gryo_sensor_data.x / 32768.0 * 1000 * 3;
			gryo[0].gyro_y_deg = gryo_sensor_data.y / 32768.0 * 1000 * 3;
			gryo[0].gyro_z_deg = gryo_sensor_data.z / 32768.0 * 1000 * 3;
		}	
		
		/* gryoscope result */
		printf("gryo x is %d, gryo deg is %f deg/s.\n", gryo_sensor_data.x, gryo[0].gyro_x_deg);
		printf("gryo y is %d, gryo deg is %f deg/s.\n", gryo_sensor_data.y, gryo[0].gyro_y_deg);
		printf("gryo z is %d, gryo deg is %f deg/s.\n", gryo_sensor_data.z, gryo[0].gyro_z_deg);

		usleep(200 * 1000);	/* 200ms */
	}
}
